import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from general_interface.msg import ControlSignal,ChassisServo,WorkMode
from general_interface.srv import GetAllInstances
from general_interface.action import SwitchMode, SwitchConnection
from lifecycle_msgs.srv import ChangeState
from lifecycle_msgs.msg import Transition


class ROS2Node(Node):
    def __init__(self, quit_func):
        self.csName = "/control_signal"
        super().__init__("command_central")
        self.inputSub = self.create_subscription(
            ChassisServo, self.csName, self.inputCallback, 10
        )
        self.inputPub = None

        self.getAllInstanceClient = self.create_client(
            GetAllInstances, "/get_all_instances"
        )
        self.coreClient = {}
        self.autoDockClient = self.create_client(ChangeState, "/auto_dock/change_state")
        self.disconnectClient = None

        self.quit_func = quit_func

    def onTick(self):
        if not rclpy.ok():
            self.quit_func()
        rclpy.spin_once(self, timeout_sec=0.05)

    def onDestory(self):
        rclpy.shutdown()

    def Info(self, mes: str):
        self.get_logger().info(mes)

    def BalanceMode(self, robot: str):
        self.ChangeCoreMode(robot, WorkMode.BALANCE)

    def RCMode(self, robot: str, enable: bool):
        if enable:
            self.inputPub = self.create_publisher(
                ChassisServo, robot + self.csName, 10
            )
            self.ChangeCoreMode(robot, WorkMode.STATESERVO)
        else:
            self.destroy_publisher(self.inputPub)
            self.inputPub = None
            self.ChangeCoreMode(robot, WorkMode.BALANCE)

    def AutoDockMode(self, robot1: str, robot2: str, enable: bool):
        if enable:
            req = ChangeState.Request()
            req.transition.id = Transition.TRANSITION_ACTIVATE
            self.autoDockClient.call_async(req)

        else:
            req = ChangeState.Request()
            req.transition.id = Transition.TRANSITION_DEACTIVATE
            self.autoDockClient.call_async(req)

    def DisconnectMode(self, robot1: str):
        self.disconnectClient = ActionClient(
            self, SwitchConnection, f"/{robot1}/switch_connection"
        )
        req: SwitchConnection.Goal = SwitchConnection.Goal()
        req.command = SwitchConnection.Goal.DISCONNECT
        self.disconnectClient.wait_for_server()
        self.disconnectClient.send_goal_async(req)

    def CloseControl(self, robot1: str):
        self.ChangeCoreMode(robot1, WorkMode.IDLE)

    def GetAllInstances(self) -> list[tuple[str, int]]:
        future = self.getAllInstanceClient.call_async(GetAllInstances.Request())
        rclpy.spin_until_future_complete(self, future)
        response: GetAllInstances.Response = future.result()
        ret = []
        for inst in response.instances:
            ret.append((inst.name, inst.qr_code))
        return ret

    def EnsureNodeCoreOnline(self, name: str):
        if self.coreClient.__contains__(name):
            pass
        else:
            self.coreClient[name] = ActionClient(
                self, SwitchMode, f"/{name}/switch_mode"
            )
            self.get_logger().info(f"{name} core online")

    def ChangeCoreMode(self, name: str, mode: int):
        self.EnsureNodeCoreOnline(name)
        client = self.coreClient[name]
        goal = SwitchMode.Goal()
        goal.mode.mode = mode
        client.wait_for_server()
        future = client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, future)

    def inputCallback(self, msg: ChassisServo):
        if self.inputPub:
            self.inputPub.publish(msg)
